#!/usr/bin/env python
print("importing libraries")

# Schweizer-Messer (swiss-army knife utils)
from matplotlib.offsetbox import DEBUG
import sm
from sm import PlotCollection

# Kalibr Modules
import aslam_cv_backend as acvb
import kalibr_common as kc
import kalibr_camera_calibration as kcc

# Python modules
import copy
import os
import numpy as np
import multiprocessing
import pylab as pl
import argparse
import sys
import random
import signal
import dill
import scipy.io
import random

from tartan_logging import TartanLogging

np.set_printoptions(suppress=True)

def initBagDataset(bagfile, topic, from_to):
    """ Reads a ROS bag dataset into a reader and returns the reader."""
    print("\tDataset:          {0}".format(bagfile))
    print("\tTopic:            {0}".format(topic))
    reader = kc.BagImageDatasetReader(bagfile, topic, bag_from_to=from_to)
    print("\tNumber of images: {0}".format(reader.numImages()))
    return reader

num_corners = np.array([])
log_file = 'log.txt'
force_all_points = False

# List of tartanlogging classes, these will be saved to a pickle for analysis at a later time
tartanlogs = []

# Camera models available to choose from.
cameraModels = { 'pinhole-radtan': acvb.DistortedPinhole,
                 'pinhole-equi':   acvb.EquidistantPinhole,
                 'pinhole-fov':    acvb.FovPinhole,
                 'omni-none':      acvb.Omni,
                 'omni-radtan':    acvb.DistortedOmni,
                 'eucm-none':      acvb.ExtendedUnified,
                 'ds-none':        acvb.DoubleSphere}

DEBUG_MODES = [
    "pointcloudprojection",
    "pinholeprojection",
    "originalprojection",
    "targetpointcloud",
    "individualprojections",
    "reprojectionpoints",
    "none"
]

REPROJECTION_MODES = [
    "pinhole",
    "homography",
    "cornerpredictor",
    "none"
]

DEFAULT_FOV = [[90],[90]]
DEFAULT_POSE = [[90],[0]]
DEFAULT_RESOLUTION = [[1000],[1000]]
DEFAULT_PROJECTION = ["cornerpredictor"]
DEFAULT_DEBUG_MODES = ["none"]

def signal_exit(signal, frame):
    sm.logWarn("Shutdown requested! (CTRL+C)")
    sys.exit(2)

def parseArgs():
    """ Parse arguments used within TartanCalib."""
    # Class for parsing Kalibr arguments.
    class KalibrArgParser(argparse.ArgumentParser):
        def error(self, message):
            self.print_help()
            sm.logError('%s' % message)
            sys.exit(2)
        def format_help(self):
            formatter = self._get_formatter()
            formatter.add_text(self.description)
            formatter.add_usage(self.usage, self._actions,
                                self._mutually_exclusive_groups)
            for action_group in self._action_groups:
                formatter.start_section(action_group.title)
                formatter.add_text(action_group.description)
                formatter.add_arguments(action_group._group_actions)
                formatter.end_section()
            formatter.add_text(self.epilog)
            return formatter.format_help()

    usage = """
    Example usage to calibrate a camera system with two cameras using an aprilgrid.

    cam0: omnidirection model with radial-tangential distortion
    cam1: pinhole model with equidistant distortion
    
    %(prog)s --models omni-radtan pinhole-equi --target aprilgrid.yaml \\
              --bag MYROSBAG.bag --topics /cam0/image_raw /cam1/image_raw

    example aprilgrid.yaml:
        target_type: 'aprilgrid'
        tagCols: 6
        tagRows: 6
        tagSize: 0.088  #m
        tagSpacing: 0.3 #percent of tagSize"""
    parser = KalibrArgParser(description='Calibrate the intrinsics and extrinsics of a camera system with non-shared overlapping field of view.', usage=usage)
    parser.add_argument('--models', nargs='+', dest='models', help='The camera model {0} to estimate'.format(list(cameraModels.keys())), required=True)

    groupSource = parser.add_argument_group('Data source')
    groupSource.add_argument('--bag', dest='bagfile', help='The bag file with the data')
    groupSource.add_argument('--topics', nargs='+', dest='topics', help='The list of image topics', required=True)
    groupSource.add_argument('--bag-from-to', metavar='bag_from_to', type=float, nargs=2, help='Use the bag data starting from up to this time [s]')

    groupTarget = parser.add_argument_group('Calibration target configuration')
    groupTarget.add_argument('--target', dest='targetYaml', help='Calibration target configuration as yaml file', required=True)

    groupImageSync = parser.add_argument_group('Image synchronization')
    groupImageSync.add_argument('--approx-sync', dest='max_delta_approxsync', type=float, default=0.02, help='Time tolerance for approximate image synchronization [s] (default: %(default)s)')

    groupCalibrator = parser.add_argument_group('Calibrator settings')
    groupCalibrator.add_argument('--qr-tol', type=float, default=0.02, dest='qrTol', help='The tolerance on the factors of the QR decomposition (default: %(default)s)')
    groupCalibrator.add_argument('--mi-tol', type=float, default=0.2, dest='miTol', help='The tolerance on the mutual information for adding an image. Higher means fewer images will be added. Use -1 to force all images. (default: %(default)s)')
    groupCalibrator.add_argument('--no-shuffle', action='store_true', dest='noShuffle', help='Do not shuffle the dataset processing order')

    groupTartan = parser.add_argument_group("Tartan settings")
    groupTartan.add_argument('--fovs', type=int,dest='l_fov', nargs='+', action='append', help='If using pinhole projection mode, this parameter represents FOV of pinhole')
    groupTartan.add_argument('--poses', type=int,dest='l_pose', nargs='+', action='append', help='If using pinhole projection mode, this parameter represents pose of pinhole')
    groupTartan.add_argument('--resolutions', type=int, dest='l_res', nargs='+', action='append', help='If using pinhole projection mode, this parameter represents resolution of pinhole')
    groupTartan.add_argument('--projections', type=str, dest='l_proj', nargs='*', action='append', help='Choose from four possible projections: pinhole, homography, cornerpredictor and none. Cornerpredictor is the autocomplete method described within the paper')
    groupTartan.add_argument('--debug-modes', type=str, dest='l_debug', nargs='*', action='append', help='Choose from 7 possible debug modes for additional debug images: pointcloudprojection, pinholeprojection, originalprojection, targetpointcloud, individualprojections, reprojectionpoints and none.')
    groupTartan.add_argument('--min-init-corners-autocomplete', type=int, default=24, dest='min_init_corners_autocomplete', help='Minimum number of corners for autocomplete, since pose of the board is otherwise too inaccurate.')
    groupTartan.add_argument('--min-tag-size-autocomplete', type=int, default=0, dest='min_tag_size_autocomplete', help='Minimum tag size (px) before autocomplete. To ensure really small tags are discarded instead of detected inaccurately.')
    groupTartan.add_argument('--correction-threshold', type=float, default=2.0, dest='correction_threshold', help='Number of pixels allowed for offset between reprojection and detection.')
    groupTartan.add_argument('--min-resize-window-size', type=int, default=2, dest='min_resize_window_size', help='Minimum window size allowed during dynamic sizing')
    groupTartan.add_argument('--max-resize-window-size', type=int, default=8, dest='max_resize_window_size', help='Maximum window size allowed during dynamic sizing')
    groupTartan.add_argument('--refine-magnitude-reject', type=float, default=2.0, dest='refine_magnitude_reject', help='')
    groupTartan.add_argument('--symmetry-refinement', action='store_true', dest='symmetry_refinement', help='Include symmetry refinement method')
    groupTartan.add_argument('--symmetry-edge-threshold', type=float, default=10.0, dest='symmetry_edge_threshold', help='')
    groupTartan.add_argument('--export-dataset-dir', type=str, default="", dest="export_dataset_dir", help='Filepath to export binary usable in generic camera model referenced in "Why Having 10000..." paper.')

    outlierSettings = parser.add_argument_group('Outlier filtering options')
    outlierSettings.add_argument('--no-outliers-removal', action='store_false', default=True, dest='removeOutliers', help='Disable corner outlier filtering')
    outlierSettings.add_argument('--no-final-filtering', action='store_false', default=True, dest='allowEndFiltering', help='Disable filtering after all views have been processed.')
    outlierSettings.add_argument('--min-views-outlier', type=int, default=20, dest='minViewOutlier', help='Number of raw views to initialize statistics (default: %(default)s)')
    outlierSettings.add_argument('--use-blakezisserman', action='store_true', dest='doBlakeZisserman', help='Enable the Blake-Zisserman m-estimator')
    outlierSettings.add_argument('--plot-outliers', action='store_true', dest='doPlotOutliers', help='Plot the detect outliers during extraction (this could be slow)')

    outputSettings = parser.add_argument_group('Output options')
    outputSettings.add_argument('--verbose', action='store_true', dest='verbose', help='Enable (really) verbose output (disables plots)')
    outputSettings.add_argument('--show-extraction', action='store_true', dest='showextraction', help='Show the calibration target extraction. (disables plots)')
    outputSettings.add_argument('--plot', action='store_true', dest='plot', help='Plot during calibration (this could be slow).')
    outputSettings.add_argument('--dont-show-report', action='store_true', dest='dontShowReport', help='Do not show the report on screen after calibration.')
    outputSettings.add_argument('--polar_setting', type=int, default=0, dest='polarSetting', help='The way we are using polar angles of detections to improve results. 0=None, don not use. 1=set an angle threshold, under this threshold no corners will be used. 2 = bins, divide the corners into polar angle bins and give each bin a weight.')
    outputSettings.add_argument('--force_all', action='store_true', dest='forceAll', help='Force all points to be used.')
    outputSettings.add_argument('--polar_cutoff', type=float, dest='polarCutoff',default=50, help='Binary cutoff for polar.')
    outputSettings.add_argument('--outputMatlab',action='store_true',dest='outputMatlab',help='Outputs a matlab matrix for use in Babelcalib.')
    outputSettings.add_argument('--save_dir', type=str, dest='saveDir',default='/data/', help='All the data will be logged to this directory.')
    outputSettings.add_argument('--log_dest', type=str, dest='logDest',default='log', help='Name (prefix) of log files.')
    outputSettings.add_argument('--debug_image_dir', type=str, dest='debugImageDir', default='', help='Directory for saving debug images for analysis.')

    # Print help if no argument is specified
    if len(sys.argv)==1:
        parser.print_help()
        sys.exit(2)

    # Parse the argument list
    try:
        parsed = parser.parse_args()
    except:
        sys.exit(2)

    # Checks required for TartanCalib to run.
    if len(parsed.topics) != len(parsed.models):
        sm.logError("Please specify exactly one camera model (--models) for each topic (--topics).")
        sys.exit(2)

    if parsed.minViewOutlier<1:
        sm.logError("Please specify a positive integer (--min-views-outlier).")
        sys.exit(2)

    # There is an issue with the gtk plot widget where we cannot plot if we have opencv windows open.
    # Disable plots in this special situation.
    if parsed.showextraction or parsed.verbose:
        parsed.dontShowReport = True

    return parsed

def main():
    parsed = parseArgs()

    # Perform check for saveDir / debugImageDir upfront.
    if (not os.path.exists(parsed.saveDir)):
        try:
            os.makedirs(parsed.saveDir)
        except:
            print("[ERROR] Unable to create directory in:{}".format(parsed.saveDir))
            raise(OSError)

    if (parsed.debugImageDir != "" and not os.path.exists(parsed.debugImageDir)):
        try:
            os.makedirs(parsed.debugImageDir)
        except:
            print("[ERROR] Unable to create directory in:{}".format(parsed.debugImageDir))
            raise(OSError)

    # Logging modes (verbosity)
    if parsed.verbose:
        sm.setLoggingLevel(sm.LoggingLevel.Debug)
    else:
        sm.setLoggingLevel(sm.LoggingLevel.Info)

    # Argparse has issues with default value being a list which requires additional attention (https://bugs.python.org/issue16399).
    if parsed.l_fov is None:
        parsed.l_fov = DEFAULT_FOV
    if parsed.l_pose is None:
        parsed.l_pose = DEFAULT_POSE
    if parsed.l_res is None:
        parsed.l_res = DEFAULT_RESOLUTION
    
    # Un-nesting list if arguments were supplied from argparser as argparser always nests list.
    if parsed.l_proj is None:
        projections = DEFAULT_PROJECTION
    else:
        projections = parsed.l_proj[0]
    if parsed.l_debug is None:
        debug_modes = DEFAULT_DEBUG_MODES
    else:
        debug_modes = parsed.l_debug[0]

    # Setting up numpy arrays for TartanCalibWorker
    # TODO: Implement checks for number of values / if np arrayable.
    fovs = np.array(parsed.l_fov)
    poses = np.array(parsed.l_pose)
    resolutions = np.array(parsed.l_res)

    # Checking for validity in projections / debug_modes.
    for projection in projections:
        assert(projection in REPROJECTION_MODES)    
    for debug in debug_modes:
        assert(debug in DEBUG_MODES)

    #register signal handler
    signal.signal(signal.SIGINT, signal_exit)

    targetConfig = kc.CalibrationTargetParameters(parsed.targetYaml)

    #create camera objects, initialize the intrinsics and extract targets
    cameraList = list()
    numCams = len(parsed.topics)

    obsdb = kcc.ObservationDatabase(parsed.max_delta_approxsync)
    obslist = list()

    for cam_id in range(numCams):
        topic = parsed.topics[cam_id]
        modelName = parsed.models[cam_id]
        print("Initializing cam{0}:".format(cam_id))
        print("\tCamera model:\t  {0}".format(modelName))

        if modelName in cameraModels:
            #open dataset
            dataset = initBagDataset(parsed.bagfile, topic, parsed.bag_from_to)

            #create camera
            cameraModel = cameraModels[modelName]
            cam = kcc.CameraGeometry(cameraModel, targetConfig, dataset, verbose=(parsed.verbose or parsed.showextraction))

            #extract the targets
            multithreading = not (parsed.verbose or parsed.showextraction)
            observations = kc.extractCornersFromDataset(cam.dataset, cam.ctarget.detector,
                                                        multithreading=multithreading, clearImages=False,
                                                        noTransformation=True)
            #populate the database
            for obs in observations:
                obsdb.addObservation(cam_id, obs)

            obslist.append(observations)
            #initialize the intrinsics
            if not cam.initGeometryFromObservations(observations):
                raise RuntimeError("Could not initialize the intrinsics for camera with topic: {0}. Try to use --verbose and check whether the calibration target extraction is successful.".format(topic))

            print("\tProjection initialized to: %s" % cam.geometry.projection().getParameters().flatten())
            print("\tDistortion initialized to: %s" % cam.geometry.projection().distortion().getParameters().flatten())



            cameraList.append(cam)
        else:
            raise RuntimeError( "Unknown camera model: {0}. Try {1}.".format(modelName, list(cameraModels.keys())) )

    if parsed.verbose:
        obsdb.printTable()

    #initialize the calibration graph
    graph = kcc.MulticamCalibrationGraph(obsdb)

    if not graph.isGraphConnected():
        obsdb.printTable()
        print("Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.")
        graph.plotGraph()
        sys.exit(-1)

    #loop to restart the optimization

    obslistTartan = copy.deepcopy(obslist)

    num_iterations = 2
    for iteration in range(num_iterations):
        restartAttempts=100
        initOutlierRejection=True
        removedOutlierCorners=list()

        while True:
            try:
                obsdb_init = copy.deepcopy(obsdb)
                #compute initial guesses for the baselines, intrinsics
                print("initializing initial guesses")
                if len(cameraList)>1:
                    baseline_guesses = graph.getInitialGuesses(cameraList)
                else:
                    baseline_guesses=[]

                if parsed.verbose and len(cameraList)>1:
                    graph.plotGraph()

                for baseline_idx, baseline in enumerate(baseline_guesses):
                    print("initialized baseline between cam{0} and cam{1} to:".format(baseline_idx, baseline_idx+1))
                    print(baseline.T())

                for cam_idx, cam in enumerate(cameraList):
                    print("initialized cam{0} to:".format(cam_idx))
                    print("\t projection cam{0}: {1}".format(cam_idx, cam.geometry.projection().getParameters().flatten()))
                    print("\t distortion cam{0}: {1}".format(cam_idx, cam.geometry.projection().distortion().getParameters().flatten()))


                print("initializing calibrator")
                polar_object = kcc.PolarWeighting()

                # if iteration > 0:
                if parsed.polarSetting == 1:
                    polar_object.mode = kcc.PolarOptions.CUTOFF
                    polar_object.binaryCutOff = parsed.polarCutoff

                elif parsed.polarSetting == 2:
                    polar_object.mode = kcc.PolarOptions.RAND_BIN_PICK
                    polar_object.numBins = 50

                calibrator = kcc.CameraCalibration(cameraList, baseline_guesses, verbose=parsed.verbose, useBlakeZissermanMest=parsed.doBlakeZisserman,polarObject=polar_object)

                if polar_object.mode == kcc.PolarOptions.RAND_BIN_PICK :
                    calibrator.getPolarDistribution(obsdb,kcc.CalibrationTarget(cameraList[0].ctarget.detector.target()),cameraList)

                options = calibrator.estimator.getOptions()
                options.infoGainDelta = parsed.miTol
                options.checkValidity = True
                options.verbose = parsed.verbose
                linearSolverOptions = calibrator.estimator.getLinearSolverOptions()
                linearSolverOptions.columnScaling = True
                linearSolverOptions.verbose = parsed.verbose
                linearSolverOptions.epsSVD = 1e-6
                #linearSolverOptions.svdTol = 0.0 #TODO
                #linearSolverOptions.qrTol = 0.0

                optimizerOptions = calibrator.estimator.getOptimizerOptions()
                optimizerOptions.maxIterations = 50
                optimizerOptions.nThreads = max(1,multiprocessing.cpu_count()-1)
                optimizerOptions.verbose = parsed.verbose
                verbose = parsed.verbose

                doPlot = parsed.plot
                if doPlot:
                    print("Plotting during calibration. Things may be very slow (but you might learn something).")

                #shuffle the views
                timestamps = obsdb.getAllViewTimestamps()
                if not parsed.noShuffle:
                    random.shuffle(timestamps)

                #process all target views
                print("starting calibration...")
                numViews = len(timestamps)
                progress = sm.Progress2(numViews); progress.sample()
                for view_id, timestamp in enumerate(timestamps):
                    #add new batch problem
                    obs_tuple = obsdb.getAllObsAtTimestamp(timestamp)
                    est_baselines = list()
                    for bidx, baseline in enumerate(calibrator.baselines):
                        est_baselines.append( sm.Transformation(baseline.T()) )
                    T_tc_guess = graph.getTargetPoseGuess(timestamp, cameraList, est_baselines)
                    success = False
                    success = calibrator.addTargetView(obs_tuple, T_tc_guess,parsed.forceAll)



                    #display process
                    if (verbose or (view_id % 25) == 0) and calibrator.estimator.getNumBatches()>0 and view_id>1:
                        print("")
                        print("------------------------------------------------------------------")
                        print("")
                        print("Processed {0} of {1} views with {2} views used".format(view_id+1, numViews, calibrator.estimator.getNumBatches()))
                        print("")
                        kcc.printParameters(calibrator)
                        print("")
                        print("------------------------------------------------------------------")

                    #calibration progress
                    progress.sample()

                    #plot added views
                    if success and doPlot:
                        recent_view = calibrator.views[-1]
                        cams_in_view = [obs_tuple[0] for obs_tuple in recent_view.rig_observations]
                        plotter = PlotCollection.PlotCollection("Added view (stamp: {0})".format(timestamp))
                        for cam_id in cams_in_view:
                            fig=pl.figure(view_id*5000+cam_id)
                            kcc.plotAllReprojectionErrors(calibrator, cam_id, fno=fig.number, noShow=True)
                            plotter.add_figure("cam{0}".format(cam_id), fig)
                        plotter.show()

                    # Look for outliers
                    runEndFiltering = view_id==(len(timestamps)-1) and parsed.allowEndFiltering # run another filtering step at the end (over all batches)
                    numActiveBatches = calibrator.estimator.getNumBatches()
                    if ((success and numActiveBatches>parsed.minViewOutlier*numCams) or (runEndFiltering and numActiveBatches>parsed.minViewOutlier*numCams)) and parsed.removeOutliers:
                        #create the list of the batches to check
                        if initOutlierRejection:
                            #check all views after the min. number of batches has been reached
                            batches_to_check=list(range(0, calibrator.estimator.getNumBatches()))
                            print("");print("")
                            print("Filtering outliers in all batches...")
                            initOutlierRejection=False
                            progress_filter = sm.Progress2(len(batches_to_check)); progress_filter.sample()
                        elif runEndFiltering:
                            #check all batches again after all views have been processed
                            print("");print("")
                            print("All views have been processed.\n\nStarting final outlier filtering...")
                            batches_to_check=list(range(0, calibrator.estimator.getNumBatches()))
                            progress_filter = sm.Progress2(len(batches_to_check)); progress_filter.sample()
                        else:
                            #only check most recent view
                            batches_to_check = [ calibrator.estimator.getNumBatches()-1 ]

                        #now check all the specified batches
                        batches_to_check.sort()
                        batches_to_check.reverse()
                        for batch_id in batches_to_check:

                            #check all cameras in this batch
                            cornerRemovalList_allCams=list()
                            camerasInBatch = list(calibrator.views[batch_id].rerrs.keys())
                            for cidx in camerasInBatch:

                                # Calculate the reprojection errors statistics
                                corners, reprojs, rerrs = kcc.getReprojectionErrors(calibrator, cidx)
                                me, se = kcc.getReprojectionErrorStatistics(rerrs)
                                se_threshold = 4.0*se #TODO: find good value

                                #select corners to remove
                                cornerRemovalList=list()
                                for pidx, reproj in enumerate(rerrs[batch_id]):
                                    if (not np.all(reproj==np.array([None,None]))) and (abs(reproj[0]) > se_threshold[0] or abs(reproj[1]) > se_threshold[1]):
                                        cornerRemovalList.append(pidx)

                                        #display the corners info
                                        if parsed.verbose or parsed.doPlotOutliers:
                                            sm.logInfo( "Outlier detected on view {4} with idx {5} (rerr=({0}, {1}) > ({2},{3}) )".format(reproj[0], reproj[1], se_threshold[0], se_threshold[1], view_id, pidx))
                                            sm.logInfo( "Predicted: {0}".format(calibrator.views[batch_id].rerrs[cidx][pidx].getPredictedMeasurement()) )
                                            sm.logInfo( "Measured: {0}".format(calibrator.views[batch_id].rerrs[cidx][pidx].getMeasurement()) )

                                        #store the outlier corners for plotting
                                        removedOutlierCorners.append( (cidx, calibrator.views[batch_id].rerrs[cidx][pidx].getMeasurement()) )

                                #queue corners on this cam for removal
                                cornerRemovalList_allCams.append( (cidx, cornerRemovalList) )

                                #plot the observation with the outliers
                                if len(cornerRemovalList)>0 and parsed.doPlotOutliers:
                                    for cam_id, obs in calibrator.views[batch_id].rig_observations:
                                        if cam_id==cidx:
                                            gridobs = obs
                                    fig=pl.figure(view_id*100+batch_id+cidx)
                                    kcc.plotCornersAndReprojection(gridobs, reprojs[batch_id], cornerlist=cornerRemovalList,
                                                                fno=fig.number, clearFigure=True, plotImage=True,
                                                                title="Removing outliers in view {0} on cam {0}".format(view_id, cidx))
                                    pl.show()

                            #remove the corners (if there are corners to be removed)
                            removeCount = sum([len(removelist) for cidx, removelist in cornerRemovalList_allCams])
                            if removeCount>0:
                                original_batch = calibrator.views[batch_id]
                                new_batch = kcc.removeCornersFromBatch(original_batch, cornerRemovalList_allCams, useBlakeZissermanMest=parsed.doBlakeZisserman,polarObject=polar_object)

                                #replace the original batch with the corrected
                                calibrator.estimator.removeBatch( calibrator.views[batch_id] )
                                calibrator.views[batch_id] = new_batch
                                rval = calibrator.estimator.addBatch( new_batch, parsed.forceAll )

                                #queue the batch for removal if the corrected batch was rejected
                                if not rval.batchAccepted:
                                    sm.logDebug("corrected view rejected! removing from optimization...")
                                    calibrator.views.remove( calibrator.views[batch_id] )
                                sm.logDebug("Removed {0} outlier corners on batch {1}".format(removeCount, batch_id))

                            #start and end filtering progress bar
                            if len(batches_to_check)>1:
                                progress_filter.sample()



                #final output
                print("")
                print("")
                print("..................................................................")
                print("")
                print("Calibration complete.")

                #obsdb_init is the obsdb that was used to train this model
                tartanlogs.append(TartanLogging.TartanLogger(obsdb_init,[copy.deepcopy(cam_.geometry) for cam_ in calibrator.cameras],kcc.getAllPointStatistics(calibrator,0)))


                if (iteration < (num_iterations-1)):
                    obsdb = kcc.ObservationDatabase(parsed.max_delta_approxsync)

                    for cam_id in range(0, numCams):
                        cam = cameraList[cam_id]
                        # This passes all configuration arguments and calls the TartanCalibWorker through CameraGeometry.getProjections
                        # TODO: Parameters should ideally be passed as a struct.
                        tartanParams = [parsed.min_init_corners_autocomplete,
                            parsed.min_tag_size_autocomplete,
                            parsed.correction_threshold,
                            parsed.min_resize_window_size,
                            parsed.max_resize_window_size,
                            parsed.refine_magnitude_reject,
                            parsed.symmetry_refinement,
                            parsed.symmetry_edge_threshold,
                            parsed.export_dataset_dir,
                            parsed.debugImageDir
                        ]
                        
                        new_obs = cam.geometry.getProjections(
                            cam.ctarget.detector,
                            obslistTartan[cam_id],
                            fovs,
                            poses,
                            resolutions,
                            projections,
                            debug_modes,
                            tartanParams
                        )

                        for obs in new_obs:
                            if obs.hasSuccessfulObservation(): # sometime TartanCalib rejects all original (Kalibr) features
                                obsdb.addObservation(cam_id,obs)

                        obslistTartan[cam_id] = new_obs

                    # Saves matlab matrix for use in Babelcalib if outputMatlab option is selected
                    if (parsed.outputMatlab and iteration == (num_iterations-2)):
                        print("Saving to MatLab matrix")
                        timestamps = obsdb.getAllViewTimestamps()
                        # add all observed corners
                        corners_mat = []

                        for view_id, timestamp in enumerate(timestamps):
                            #add new batch problem
                            obs_tuple = obsdb.getAllObsAtTimestamp(timestamp)
                            obs = obs_tuple[0][1]
                            #assumption: only single-cam use
                            x = obs.getCornersImageFrame()
                            x = x.transpose()
                            corner_idxs = obs.getCornersIdx()

                            num_corners = np.shape(x)[1]
                            cspond = np.ones((2,num_corners))
                            # cspond[0,:] = np.arange(1,num_corners+1)
                            # cspond[1,:] = corner_idxs+1
                            cspond[0,:] = corner_idxs+1

                            corners_mat.append({"x":x,"cspond":cspond})

                        # add board definition
                        num_corners = cameraList[0].ctarget.detector.target().size()
                        x_board = np.zeros((2,num_corners))
                        for q in range(num_corners):
                            x_board[0,q] = cameraList[0].ctarget.detector.target().point(q)[0]
                            x_board[1,q] = cameraList[0].ctarget.detector.target().point(q)[1]

                        # identity because we only have one board, so it's at the centre of the global coordinate frame
                        Rt = np.array([[1.0,0,0,0],[0,1.0,0,0],[0,0,1.0,0]])

                        # save corners mat
                        corners_mat_ = np.empty((len(corners_mat),), dtype=np.object)
                        for i in range(len(corners_mat)):
                            corners_mat_[i] = corners_mat[i]
                        scipy.io.savemat('corners.mat',{"corners":corners_mat})

                        # save board def mat
                        scipy.io.savemat('board.mat',{"boards":{"Rt":Rt,"X":x_board}})

                # Initialize the calibration graph
                graph = kcc.MulticamCalibrationGraph(obsdb)

                print("")
                if parsed.removeOutliers:
                    sm.logWarn("Removed {0} outlier corners.".format(len(removedOutlierCorners)) )
                print("")
                print("Processed {0} images with {1} images used".format(numViews, calibrator.estimator.getNumBatches()))
                kcc.printParameters(calibrator)

                if parsed.verbose and len(calibrator.baselines)>1:
                    f=pl.figure(100006)
                    kcc.plotCameraRig(calibrator.baselines, fno=f.number, clearFigure=False)
                    pl.show()

                # Write results to file
                resultFile = os.path.join(parsed.saveDir, parsed.logDest + str(iteration)  + "-camchain.yaml")
                kcc.saveChainParametersYaml(calibrator, resultFile, graph)
                print("Results written to file: {0}".format(resultFile))

                # Write detailed results to file
                resultFileTxt = os.path.join(parsed.saveDir, parsed.logDest + str(iteration) + "-results-cam.txt")
                kcc.saveResultTxt(calibrator, filename=resultFileTxt)
                print("  Detailed results written to file: {0}".format(resultFileTxt))

                # Generate report
                reportFile = os.path.join(parsed.saveDir, parsed.logDest + str(iteration) +"-report-cam.pdf")
                G=None
                if numCams>1:
                    G=graph

                with open(os.path.join(parsed.saveDir, parsed.logDest + '.pkl'), 'wb') as f:
                    dill.dump(tartanlogs, f)

            except kcc.OptimizationDiverged:
                restartAttempts-=1
                sm.logWarn("Optimization diverged possibly due to a bad initialization. (Do the models fit the lenses well?)")

                if restartAttempts==0:
                    sm.logError("Max. attempts reached... Giving up...")
                    break
                else:
                    sm.logWarn("Restarting for a new attempt...")

                    #reinitialize the intrinsics
                    for cam_id, cam in enumerate(cameraList):
                        print("Reinitialize the intrinsics for camera {0}".format(cam_id))
                        observations = obsdb.getAllObsCam(cam_id)
                        if not cam.initGeometryFromObservations(observations):
                            raise RuntimeError("Could not re-initialize the intrinsics for camera with topic: {0}".format(topic))

                        print("\tProjection initialized to: %s" % cam.geometry.projection().getParameters().flatten())
                        print("\tDistortion initialized to: %s" % cam.geometry.projection().distortion().getParameters().flatten())

            else:
                break #normal exit

    kcc.generateReport(calibrator, reportFile, showOnScreen=not parsed.dontShowReport, graph=G, removedOutlierCorners=removedOutlierCorners)

if __name__ == "__main__":
    main()
#     try:
#         main()
#     except Exception,e:
#         sm.logError("Exception: {0}".format(e))
#         sys.exit(-1)
